#pragma once

#include <algorithm/math.h>
#include <complex>
#include <core.h>

namespace os::filter {
/**
 * @brief 通用IIR低通滤波器
 * @tparam order 阶数
 * @tparam sampling_freq 采样频率——Hz
 * @tparam cutoff_freq 截止频率——Hz
 */
template <size_t order, size_t sampling_freq, size_t cutoff_freq>
class IIRLowPassFilter {
    static_assert(order >= 1, "Order must be >= 1");
    static_assert(sampling_freq > 0, "Sampling frequency must be > 0");
    static_assert(cutoff_freq > 0 && 2 * cutoff_freq < sampling_freq,
                  "Cutoff_freq must be > 0 and < sampling_freq / 2");

public:
    using CoefficientArray = std::array<float, order + 1>;

    /**
     * @param b,a matlab: [b, a] = butter(order, cutoff_freq/(sampling_freq/2));
     */
    IIRLowPassFilter(CoefficientArray const& b, CoefficientArray const& a) : b_(b), a_(a) {
        os_assert(std::abs(a_[0]) > 1e-6f, "a[0] must not be zero");

        // 归一化 a[0]
        if (std::abs(a_[0] - 1.0f) > 1e-6f) {
            float a0 = a_[0];
            for (size_t i = 0; i <= order; ++i) {
                b_[i] /= a0;
                a_[i] /= a0;
            }
        }

        reset();
    }

    /**
     * @brief 自动计算参数
     * 调参时方便，参数稳定后建议用第一个构造函数
     */
    IIRLowPassFilter() {
        butter(b_, a_);
        reset();
    }

    float calculate(float input_sample) {
        // 更新输入历史
        for (size_t i = order; i > 0; --i) x_hist_[i] = x_hist_[i - 1];
        x_hist_[0] = input_sample;

        // 计算输出
        float output = 0.0f;
        for (size_t i = 0; i <= order; ++i) output += b_[i] * x_hist_[i];
        for (size_t i = 1; i <= order; ++i) output -= a_[i] * y_hist_[i - 1];

        // 更新输出历史
        for (size_t i = order - 1; i > 0; --i) y_hist_[i] = y_hist_[i - 1];
        y_hist_[0] = output;

        return output;
    }

    void reset() {
        x_hist_.fill(0.0f);
        y_hist_.fill(0.0f);
    }

    static constexpr size_t getCutoffFrequency() { return cutoff_freq; }
    static constexpr size_t getOrder() { return order; }
    static constexpr size_t getSamplingRate() { return sampling_freq; }

private:
    CoefficientArray b_{}, a_{};
    std::array<float, order + 1> x_hist_{};
    std::array<float, order> y_hist_{};

    /**
     * @brief 计算低通巴特沃斯滤波器系数
     */
    void butter(CoefficientArray& b, CoefficientArray& a) {
        using cd = std::complex<float>;

        float const k = math::tan(units::make_unit<units::angle::radian_t>(PI * static_cast<float>(cutoff_freq) /
                                                                           static_cast<float>(sampling_freq)));

        std::array<cd, order + 1> complex_a{};
        complex_a[0] = cd(1.0f, 0.0f);
        std::size_t deg = 0;

        for (std::size_t m = 0; m < order; ++m) {
            float const theta = PI * (2.0f * static_cast<float>(m) + 1.0f + static_cast<float>(order)) /
                                (2.0f * static_cast<float>(order));
            cd const p = std::polar(1.0f, theta);
            cd const z = (cd(1.0f, 0.0f) + k * p) / (cd(1.0f, 0.0f) - k * p);

            ++deg;
            cd const neg_z = -z;
            for (std::size_t r = deg; r > 0; --r) {
                complex_a[r] += neg_z * complex_a[r - 1];
            }
        }

        b[0] = 1.0;
        for (std::size_t i = 1; i <= order; ++i) {
            b[i] = b[i - 1] * static_cast<float>(order - (i - 1)) / static_cast<float>(i);
        }

        float sumA = 0.0;
        for (std::size_t i = 0; i <= order; ++i) sumA += static_cast<float>(complex_a[i].real());
        float sumB = 0.0;
        for (std::size_t i = 0; i <= order; ++i) sumB += b[i];
        float const g = sumA / sumB;
        for (auto& bi : b) bi *= g;

        auto const a0 = static_cast<float>(complex_a[0].real());
        float const inv_a0_f = 1.0f / static_cast<float>(a0);
        float const inv_a0 = 1.f / a0;
        for (auto& ai : complex_a) ai *= inv_a0_f;
        for (auto& bi : b) bi *= inv_a0;

        for (std::size_t i = 0; i <= order; ++i) a[i] = static_cast<double>(complex_a[i].real());
    }
};

/**
 * @brief 滑动均值滤波器
 * @tparam window_size 窗口大小
 */
template <size_t window_size>
class MovingAverageFilter {
public:
    static_assert(window_size > 0, "Window size must be greater than 0");

    MovingAverageFilter() { buffer_.fill(0.0f); }

    void add_value(float newValue) {
        sum_ -= buffer_[index_];
        buffer_[index_] = newValue;
        sum_ += newValue;
        index_ = (index_ + 1) % window_size;
        if (count_ < window_size) ++count_;
    }

    [[nodiscard]] float get_value() const {
        if (count_ == 0) return 0.0f;
        return sum_ / static_cast<float>(count_);
    }

    void reset() {
        buffer_.fill(0.0f);
        sum_ = 0.0f;
        index_ = 0;
        count_ = 0;
    }

    [[nodiscard]] size_t get_count() const { return count_; }

private:
    std::array<float, window_size> buffer_;
    std::size_t index_{};
    std::size_t count_{};
    float sum_{};
};

/*
class LowPassFilter {
public:
    explicit LowPassFilter(float const cut_off_frq) : cutoff_freq_(cut_off_frq) {
        this->filter_para_ = cut_off_frq * 0.001 / (cut_off_frq * 0.001 + 1);
        last_tick_ = 0;
    }

    LowPassFilter() = default;
    ~LowPassFilter() = default;

    // 执行低通滤波
    float calculate(float const input_sample) {
        uint64_t const period = bsp::getGlobalTime_us_i64() - last_tick_;
        last_tick_ = bsp::getGlobalTime_us_i64();
        filter_para_ = (cutoff_freq_ * 0.001) / (cutoff_freq_ * 0.001 + 1);
        if ((filter_para_ > 0) && (filter_para_ <= 1)) {
            filted_val_ = filter_para_ * input_sample + (1 - filter_para_) * filted_last_val_;
            filted_last_val_ = filted_val_;
            calc_frq_ = 1000.F / (period);
            return filted_val_;
        }
        return input_sample;
    }

private:
    float cutoff_freq_{};      ///> 截止频率
    float calc_frq_{};         ///> 采样频率
    uint64_t last_tick_{};     ///> 上一次更新的时间
    float filter_para_{};      ///> 滤波参数
    float filted_val_{};       ///> 滤波后的值
    float filted_last_val_{};  ///> 上一个滤波值
};

// 滤波器类型
enum FilterType { BAND_PASS, BAND_STOP };

class BandPassStopFilter {
private:
    std::array<float, 3> x = {0, 0, 0};  // 输入信号缓存
    std::array<float, 3> y = {0, 0, 0};  // 输出信号缓存
    float b0, b1, b2, a0, a1, a2;        // 滤波器系数

public:
    BandPassStopFilter(FilterType type, float sampleRate, float lowCut, float highCut) {
        calculateCoefficients(type, sampleRate, lowCut, highCut);
    }

    void calculateCoefficients(FilterType type, float sampleRate, float lowCut, float highCut) {
        float omega1 = 2.0f * M_PI * lowCut / sampleRate;
        float omega2 = 2.0f * M_PI * highCut / sampleRate;
        float bw = omega2 - omega1;            // 带宽
        float omega0 = sqrt(omega1 * omega2);  // 中心频率

        float alpha = std::sin(omega0) * std::sinh(std::log(2) / 2 * bw * omega0 / std::sin(omega0));

        float cos_omega0 = std::cos(omega0);
        a0 = 1.0F + alpha;

        if (type == BAND_PASS) {
            b0 = alpha;
            b1 = 0;
            b2 = -alpha;
            a0 = 1 + alpha;
            a1 = -2 * cos_omega0;
            a2 = (1 - alpha);
        }
        if (type == BAND_STOP) {
            b0 = 1.0;
            b1 = -2.0 * cos_omega0;
            b2 = 1.0;
            a0 = 1.0 + alpha;
            a1 = -2.0 * cos_omega0;
            a2 = 1.0 - alpha;
        }
        // 归一化滤波器系数
        b0 /= a0;
        b1 /= a0;
        b2 /= a0;
        a1 /= a0;
        a2 /= a0;
    }

    float process(float input) {
        x[2] = x[1];
        x[1] = x[0];
        x[0] = input;

        y[2] = y[1];
        y[1] = y[0];
        y[0] = b0 * x[0] + b1 * x[1] + b2 * x[2] - a1 * y[1] - a2 * y[2];

        return y[0];
    }
};
*/
}  // namespace os::filter